/*
 *      stone.cpp
 *      
 *      Copyright 2008 AL <skylark@skylark-desktop>
 */

#include <cmath>
#include <iostream>
#include "world.h"

/* global contact information */
//static int world_numcontacts;
//static cpContact *world_contacts;

World::World(void)
{
	/* Box2D initialization */
    b2AABB worldAABB;
	worldAABB.lowerBound.Set(-WORLD_BOUNDING_BOX_RANGE, -WORLD_BOUNDING_BOX_RANGE);
    worldAABB.upperBound.Set(WORLD_BOUNDING_BOX_RANGE, WORLD_BOUNDING_BOX_RANGE);
	b2Vec2 gravity(0.0f, 0.0f);

	world = new b2World(worldAABB, gravity, /*allow sleeping objects*/true);

	if (world == NULL)
	{
		status = sysFailure;
		return;
	}
    //space->iterations = WORLD_SOLVER_ITERATIONS;
    //space->elasticIterations = WORLD_SOLVER_ITERATIONS;
    //space->gravity = cpvzero;
    //cpSpaceAddCollisionPairFunc(space, STONE_COLLISION_TYPE_ID, STONE_COLLISION_TYPE_ID, 
    //                            StoneCollision, NULL);
	status = sysRunnable;
}

World::~World()
{
	if (world != NULL)
	{
		delete world;
	}
}

void World::step(float _time)
{

	//for(std::vector<Stone*>::iterator i = stones.begin(); i != stones.end(); i++)
	//{
		/* Chipmunks based apply gravity force */
	//	Stone& si = **i;
		//cpBodyResetForces(si.body);
	//	b2Vec2 pos_i = si.body->GetPosition();

	//	for(std::vector<Stone*>::iterator j = stones.begin(); j != stones.end(); j++)
	//	{
	//		if (i != j)
	//		{
	//			Stone& sj = **j;
	//			b2Vec2 pos_j = sj.body->GetPosition();

	//			float _dist = sqrt((pos_i.x-pos_j.x)*(pos_i.x-pos_j.x)
	//			                  +(pos_i.y-pos_j.y)*(pos_i.y-pos_j.y));
	//			float F = WORLD_GRAVITY_CONSTANT*si.body->GetMass()*sj.body->GetMass()/(_dist*_dist);

	//			b2Vec2 force;
	//			force.x = F * (pos_j.x - pos_i.x) / _dist;
	//			force.y = F * (pos_j.y - pos_i.y) / _dist;
				/* Chipmunks apply force */
	//			si.body->ApplyForce(/*F vector*/force, /*world pos of force*/pos_i);
	//			
	//		}
	//	} // for j
	//} // for i

	/* Chipmunks base solver step for entire space */
	//world_contacts = 0;
	//world_numcontacts = 0;

    //world->Step(_time, WORLD_SOLVER_ITERATIONS);

	//if (world_numcontacts > 0)
	//{
	//	std::cout << "World::collision summary: ";
	//	for(int i=0; i<world_numcontacts; i++)
	//	{
	//		std::cout << "[" << i << ":"
	//		          << "jnAcc=" << world_contacts[i].jnAcc
	//		          << ",jtAcc=" << world_contacts[i].jtAcc
	//		          << "] ";
	//	}
	//	std::cout << std::endl;
	//}
}

/*
void World::insertStone(Stone *s)
{
	if (status == sysRunnable)
	{
		stones.push_back(s);
		//s->attachCpSpace(space);
	}
}
*/

ESysStatus World::getStatus()
{
	return status;
}

/* - (non-member) - stone collision pair function for chipmunk ----- */

/*
int StoneCollision(cpShape *a, cpShape *b, cpContact *contacts, int numContacts, cpFloat normal_coef, void *data)
{
	std::cout << "StoneCollision:: a=" << (void*)a << ", b=" << (void*)b
	          << "; normal_coef=" << normal_coef << "; contacts: ";
	for(int i=0; i<numContacts; i++)
	{
		std::cout << "[" << i << ":"
				  << "p={" << contacts[i].p.x << "," << contacts[i].p.y
				  << "},n={" << contacts[i].n.x << "," << contacts[i].n.y
		          << ",dist=" << contacts[i].dist
		          << "jnAcc=" << contacts[i].jnAcc
		          << ",jtAcc=" << contacts[i].jtAcc
		          << "] ";
	}
	std::cout << std::endl;
	world_numcontacts = numContacts;
	world_contacts = contacts;

	return 1;
}
*/

